﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

#include <learn/alg/alg_interpolator.h>
#include <learn/geom/vec.h>
#include <learn/utils/output.h>

using namespace xi;
using namespace xi::geom;
using namespace xi::alg;

TEST(TestInterpolator, Newton)
{
    // 插值算法
    {
        std::mt19937 generator((unsigned int)time(nullptr)); // 使用 time(nullptr) 作为随机数生成器的种子
        std::uniform_real_distribution<double> uniform(0, 100.0); // 在 [0,1] 区间内的均匀分布

        std::vector<double> x, y;

        int N = 10;
        for (int i = 0; i < N; i++)
        {
            x.push_back(uniform(generator));
            y.push_back(uniform(generator));
        }

        // 测试插值函数和 NA 算法
        Alg_Newton_Interpolator<double> interp(x, y);
        auto f = interp.interpolate();
        for (int i = 0; i < N; i++)
        {
            // 计算相对误差，拉格朗日插值不稳定
            EXPECT_LE(std::abs(f.value(x[i]) - y[i]) / std::abs(y[i]), 1e-3);
            EXPECT_LE(std::abs(interp.value(x[i]) - y[i]) / std::abs(y[i]), 1e-3);
            EXPECT_FLOAT_EQ(interp.neville_aitken(x, y, x[i]), y[i]);
        }
    }

    // 插值算法
    {
        std::mt19937 generator((unsigned int)time(nullptr)); // 使用 time(nullptr) 作为随机数生成器的种子
        std::uniform_real_distribution<double> uniform(0, 100.0); // 在 [0,1] 区间内的均匀分布

        std::vector<double> x;
        std::vector<Vec3> y;

        int N = 10;
        for (int i = 0; i < N; i++)
        {
            x.push_back(uniform(generator));
            y.push_back(Vec3(uniform(generator), uniform(generator), uniform(generator)));
        }

        // 测试插值函数和 NA 算法
        Alg_Newton_Interpolator<Vec3, double> interp(x, y);
        auto f = interp.interpolate();
        for (int i = 0; i < N; i++)
        {
            // 计算相对误差，拉格朗日插值不稳定
            EXPECT_LE((f.value(x[i]) - y[i]).modulus() / y[i].modulus(), 1e-3);
            EXPECT_LE((interp.value(x[i]) - y[i]).modulus() / y[i].modulus(), 1e-3);
            EXPECT_LE((interp.neville_aitken(x, y, x[i]) - y[i]).modulus() / y[i].modulus(), 1e-3);
        }
    }

    // Hermite 插值
    {
        std::vector<double> x({0.1, 0.1, 1.5, 2.3, 2.3, 2.3, 4.6, 4.6, 7.9});
        std::vector<double> y({1.8, 9.36, 2.4, 0.6, 8.2, 1.9, -5.2, 5.6, 3.5});

        std::vector<double> x1({0.1, 1.5, 2.3, 4.6, 7.9});
        std::vector<std::vector<double>> y1;
        y1.push_back({1.8, 9.36});
        y1.push_back({2.4});
        y1.push_back({0.6, 8.2, 1.9});
        y1.push_back({-5.2, 5.6});
        y1.push_back({3.5});

        Alg_Newton_Interpolator<double> newton(x, y);
        auto f = newton.interpolate();
        for (int i = 0; i < x1.size(); i++)
            EXPECT_FLOAT_EQ(f.value(x1[i]), y1[i][0]);

        EXPECT_FLOAT_EQ(f.derivative().value(x1[0]), y1[0][1]);
        EXPECT_FLOAT_EQ(f.derivative().value(x1[2]), y1[2][1]);
        EXPECT_FLOAT_EQ(f.derivative().derivative().value(x1[2]), y1[2][2]);
        EXPECT_FLOAT_EQ(f.derivative().value(x1[3]), y1[3][1]);

        // std::cout << newton.m_table << std::endl;
        // std::cout << newton.m_x << std::endl;
        // std::cout << newton.m_f << std::endl;
        // std::cout << f << std::endl;

        // auto f2 = newton_interpolate(x1, y1);
        // std::cout << f2 << std::endl;
    }

    // Hermite 插值
    {
        std::vector<double> x({0.1, 0.1, 1.5, 2.3, 2.3, 2.3, 4.6, 4.6, 7.9});
        std::vector<Vec3> y;

        std::vector<double> x1({0.1, 1.5, 2.3, 4.6, 7.9});
        std::vector<std::vector<Vec3>> y1;
        y1.push_back({Vec3(1.8, 1.8, 1.8), Vec3(9.36, 9.36, 9.36)});
        y1.push_back({Vec3(2.4, 2.4, 2.4)});
        y1.push_back({Vec3(0.6, 0.6, 0.6), Vec3(8.2, 8.2, 8.2), Vec3(1.9, 1.9, 1.9)});
        y1.push_back({Vec3(-5.2, -5.2, -5.2), Vec3(5.6, 5.6, 5.6)});
        y1.push_back({Vec3(3.5, 3.5, 3.5)});

        for (int i = 0; i < y1.size(); i++)
        {
            for (int j = 0; j < y1[i].size(); j++)
                y.push_back(y1[i][j]);
        }

        Alg_Newton_Interpolator<Vec3, double> newton(x, y);
        auto f = newton.interpolate();
        for (int i = 0; i < x1.size(); i++)
            EXPECT_EQ(f.value(x1[i]), y1[i][0]);

        EXPECT_EQ(f.derivative().value(x1[0]), y1[0][1]);
        EXPECT_EQ(f.derivative().value(x1[2]), y1[2][1]);
        EXPECT_EQ(f.derivative().derivative().value(x1[2]), y1[2][2]);
        EXPECT_EQ(f.derivative().value(x1[3]), y1[3][1]);
    }

    // 递推插值
    {
        std::vector<double> y({4, 13, 28});
        std::cout << Alg_Newton_Interpolator<double>::recursion(y, 3) << std::endl;
    }
}

TEST(TestInterpolator, Barycentric)
{
    {
        auto f = [](double x) -> Vec3 { return Vec3{6.0, x, std::abs(x) + x / 2 - x * x}; };

        std::vector<double> x;
        std::vector<Vec3> y;

        int N = 10;
        for (int i = 0; i < N; i++)
        {
            x.push_back(-1 + 2 * i / (N - 1.0));
            y.push_back(f(x[i]));
        }

        // 测试插值函数
        Alg_Barycentric_Interpolator<Vec3, double> interp(x, y);
        Alg_Newton_Interpolator<Vec3, double> newton(x, y);
        // std::cout << interp.m_x << std::endl;
        // std::cout << interp.m_f << std::endl;
        // std::cout << interp.m_w << std::endl;
        for (int i = 0; i < 20; i++)
        {
            double x = -1 + 2 * i / 19.0;
            EXPECT_EQ(interp.value(x), newton.value(x));
        }
    }

    // 均匀插值和切比雪夫插值
    {
        auto f = [](double x) -> Vec3 { return Vec3{6.0, x, std::abs(x) + x / 2 - x * x}; };

        auto equidistant = Alg_Barycentric_Interpolator<Vec3, double>::equidistant(f, 20);
        auto cheby = Alg_Barycentric_Interpolator<Vec3, double>::chebyshev2(f, 20);

        // 利用重心拉格朗日插值，可以减少数值误差，均匀插值能够避免发散，切比雪夫插值可以非常稳定
        for (int i = 0; i < 20; i++)
        {
            double x = -1 + 2 * i / 19.0;
            EXPECT_LE((equidistant.value(x) - f(x)).modulus() / f(x).modulus(), 3e-3);
            EXPECT_LE((cheby.value(x) - f(x)).modulus() / f(x).modulus(), 3e-3);
        }
    }
}

TEST(TestInterpolator, Spline)
{
    {
        auto f = [](double x) { return std::abs(x) + x / 2 - x * x; };

        std::vector<double> x;
        std::vector<double> y;

        int N = 10;
        for (int i = 0; i < N; i++)
        {
            x.push_back(-1 + 2 * i / (N - 1.0));
            y.push_back(f(x[i]));
        }

        Alg_CubicSpline_Interpolator<double> interp(x, y);
        auto complete = interp.interpolate(interp.Complete, interp.Complete, 1, -1);
        auto natural = interp.interpolate(interp.Natural, interp.Natural, 0, 0);
        auto not_a_knot = interp.interpolate(interp.NotAKnot, interp.NotAKnot, 0, 0);
        auto mix = interp.interpolate(interp.Complete, interp.Natural, 1, 0);
        auto mix2 = interp.interpolate(interp.Natural, interp.NotAKnot, 0, 1);

        for (int i = 0; i < N; i++)
        {
            EXPECT_LE(std::abs(complete.value(x[i]) - f(x[i])), Numeric::distance);
            EXPECT_LE(std::abs(natural.value(x[i]) - f(x[i])), Numeric::distance);
            EXPECT_LE(std::abs(not_a_knot.value(x[i]) - f(x[i])), Numeric::distance);
            EXPECT_LE(std::abs(mix.value(x[i]) - f(x[i])), Numeric::distance);
            EXPECT_LE(std::abs(mix2.value(x[i]) - f(x[i])), Numeric::distance);
        }

        EXPECT_LE(std::abs(complete.derivative().value(x[0]) - 1), Numeric::distance);
        EXPECT_LE(std::abs(complete.derivative().value(x[N - 1]) + 1), Numeric::distance);
        EXPECT_LE(std::abs(natural.derivative().derivative().value(x[0])), Numeric::distance);
        EXPECT_LE(std::abs(natural.derivative().derivative().value(x[N - 1])), Numeric::distance);
        EXPECT_LE(std::abs(mix.derivative().value(x[0]) - 1), Numeric::distance);
        EXPECT_LE(std::abs(mix.derivative().derivative().value(x[N - 1])), Numeric::distance);
        EXPECT_LE(std::abs(mix2.derivative().derivative().value(x[0])), Numeric::distance);

        EXPECT_FLOAT_EQ(not_a_knot[1].coefficients().back(), not_a_knot[2].coefficients().back());
        EXPECT_FLOAT_EQ(not_a_knot[N - 1].coefficients().back(), not_a_knot[N - 2].coefficients().back());
        EXPECT_FLOAT_EQ(mix2[N - 1].coefficients().back(), mix2[N - 2].coefficients().back());
    }

    {
        auto f = [](double x) -> Vec3 { return Vec3{8.0, x, std::abs(x) + x / 2 - x * x}; };

        std::vector<double> x;
        std::vector<Vec3> y;

        int N = 10;
        for (int i = 0; i < N; i++)
        {
            x.push_back(-1 + 2 * i / (N - 1.0));
            y.push_back(f(x[i]));
        }

        Alg_CardinalSpline_Interpolator<3> cardinal3(y);
        auto curve3 = cardinal3.interpolate(cardinal3.Cardinal3, Vec3{1.0, 0.0, 0.0}, Vec3{1.0, 1.0, 1.0});
        for (int i = 0; i < N; i++)
            EXPECT_LE((curve3->value(i + 1) - f(x[i])).modulus(), Numeric::distance);
        auto [p1, d1] = curve3->d1(1);
        auto [p2, d2] = curve3->d1(N);
        EXPECT_LE((d1 - Vec3{1.0, 0.0, 0.0}).modulus(), Numeric::distance);
        EXPECT_LE((d2 - Vec3{1.0, 1.0, 1.0}).modulus(), Numeric::distance);

        Alg_CardinalSpline_Interpolator<3> cardinal2(y);
        auto curve2 = cardinal2.interpolate(cardinal2.Cardinal2, Vec3{1.0, 0.0, 0.0}, Vec3{1.0, 1.0, 1.0});
        for (int i = 0; i < N; i++)
            EXPECT_LE((curve2->value(i + 1.5) - f(x[i])).modulus(), Numeric::distance);
        auto p3 = curve2->d0(1);
        auto p4 = curve2->d0(N + 1);
        EXPECT_LE((p3 - Vec3{1.0, 0.0, 0.0}).modulus(), Numeric::distance);
        EXPECT_LE((p4 - Vec3{1.0, 1.0, 1.0}).modulus(), Numeric::distance);
    }
}

TEST(TestInterpolator, Bezier)
{
    {
        std::vector<Vec3> poles(4);
        poles[0] = Vec3{0.0, 0.0, 0.0};
        poles[1] = Vec3{0.0, 1.0, 0.0};
        poles[2] = Vec3{1.0, 1.0, 1.0};
        poles[3] = Vec3{1.0, 0.0, 0.0};

        Alg_Bezier_Interpolator<3> interpolator;
        interpolator.f = poles;
        interpolator.x = {0.0, 0.25, 0.5, 0.75};
        auto bez = interpolator.interpolate();

        EXPECT_LE((bez->value(0.0) - poles[0]).modulus(), Numeric::distance);
        EXPECT_LE((bez->value(0.25) - poles[1]).modulus(), Numeric::distance);
        EXPECT_LE((bez->value(0.5) - poles[2]).modulus(), Numeric::distance);
        EXPECT_LE((bez->value(0.75) - poles[3]).modulus(), Numeric::distance);
    }
}

TEST(TestInterpolator, Hermite)
{
    auto curve = Alg_Hermite_Interpolator<3>::interpolate(Vec3{0.0, 0.0, 0.0}, Vec3{1.0, 1.0, 1.0}, Vec3{1.0, 0.0, 0.0},
                                                          Vec3{0.0, 1.0, 0.0});
    EXPECT_LE((curve->value(0.0) - Vec3{0.0, 0.0, 0.0}).modulus(), Numeric::distance);
    EXPECT_LE((curve->value(1.0) - Vec3{1.0, 1.0, 1.0}).modulus(), Numeric::distance);

    auto [p1, d1] = curve->d1(0.0);
    auto [p2, d2] = curve->d1(1.0);
    EXPECT_LE((p1 - Vec3{0.0, 0.0, 0.0}).modulus(), Numeric::distance);
    EXPECT_LE((p2 - Vec3{1.0, 1.0, 1.0}).modulus(), Numeric::distance);
    EXPECT_LE(dot(d1, Vec3{1.0, 0.0, 0.0}), Numeric::distance);
    EXPECT_LE(dot(d2, Vec3{0.0, 1.0, 0.0}), Numeric::distance);
}

TEST(TestInterpolator, ClassABezier)
{
    {
        Alg_ClassABezier_Interpolator<3> interpolator;
        Vec3 T0{1.0, 0.0, 0.0};
        Vec3 T1{0.2, 1.0, 4.0};
        auto curve = interpolator.interpolate(T0, T1);

        auto [p1, d1] = curve->d1(0.0);
        auto [p2, d2] = curve->d1(1.0);
        EXPECT_LE(cross(d1, T0).modulus(), Numeric::distance);
        EXPECT_LE(cross(d2, T1).modulus(), Numeric::distance);
    }
}

TEST(TestInterpolator, CubicBSpline)
{
    auto f = [](double x) -> Vec3 { return Vec3{8.0, x, std::abs(x) + x / 2 - x * x}; };

    std::vector<double> x;
    std::vector<Vec3> y;

    int N = 10;
    for (int i = 0; i < N; i++)
    {
        x.push_back(-1 + 2 * i / (N - 1.0));
        y.push_back(f(x[i]));
    }

    Alg_CubicBSpline_Interpolator<3> bspline;

    {
        auto complete =
            bspline.interpolate(y, bspline.Complete, bspline.Complete, Vec3{1.0, 0.0, 0.0}, Vec3{1.0, 1.0, 1.0});
        auto [p1, d1] = complete->d1(complete->first_param());
        auto [p2, d2] = complete->d1(complete->last_param());
        EXPECT_LE((d1 - Vec3{1.0, 0.0, 0.0}).modulus(), Numeric::distance);
        EXPECT_LE((d2 - Vec3{1.0, 1.0, 1.0}).modulus(), Numeric::distance);
    }

    {
        auto natural =
            bspline.interpolate(y, bspline.Natural, bspline.Natural, Vec3{1.0, 0.0, 0.0}, Vec3{1.0, 1.0, 1.0});
        auto [p1, d1, dd1] = natural->d2(natural->first_param());
        auto [p2, d2, dd2] = natural->d2(natural->last_param());
        EXPECT_LE((dd1 - Vec3{1.0, 0.0, 0.0}).modulus(), Numeric::distance);
        EXPECT_LE((dd2 - Vec3{1.0, 1.0, 1.0}).modulus(), Numeric::distance);
    }

    // 曲面插值
    {
        auto f = [](double u, double v) -> Vec3 { return Vec3{u, v, sin(u + v) * cos(u - v)}; };

        int N = 10;
        int M = 15;
        std::vector<std::vector<Vec3>> points(N, std::vector<Vec3>(M));
        for (int i = 0; i < N; i++)
            for (int j = 0; j < M; j++)
                points[i][j] = f(5.0 / (N - 1) * i, 5.0 / (M - 1) * j);
        auto surface = Alg_CubicBSpline_Interpolator<3>::interpolate(points);
    }
}